//#include "common.h"
//#include <anms.h>
//#include <getMatch.h>
//#include <draw.h>
//#include "fast.h"
//#include "Marker.h"
//#include <stdlib.h>
//#include "TickMeter.h"
//
//
//
//double scaleFromHomography(const Mat &H)
//{
//	Mat invH, H2;
//	invert(H, invH);
//	SVD a(H.colRange(0, 2).rowRange(0, 2), SVD::NO_UV);
//	return 1 / sqrt(a.w.at<double>(0)*a.w.at<double>(1));
//}
//
//void transformPointsForward(vector<Point2f>&origin, const Mat &H)
//{
//	vector<Point2f> dst;
//	for (int i = 0; i<origin.size(); i++) {
//		auto &a = origin[i];
//		double *aa = (double*)H.data;
//		double s = aa[6] * a.x + aa[7] * a.y + aa[8];
//		Point2f aaa;
//		aaa.x = (aa[0] * a.x + aa[1] * a.y + aa[2]) / s;
//		aaa.y = (aa[3] * a.x + aa[4] * a.y + aa[5]) / s;
//		dst.push_back(aaa);
//	}
//	origin = dst;
//}
//
//void transformPointsInverse(vector<Point2f>&origin, const Mat &H)
//{
//	Mat invH;
//	invert(H, invH);
//	vector<Point2f> dst;
//	for (int i = 0; i<origin.size(); i++) {
//		auto &a = origin[i];
//		double *aa = (double*)invH.data;
//		double s = aa[6] * a.x + aa[7] * a.y + aa[8];
//		Point2f aaa;
//		aaa.x = (aa[0] * a.x + aa[1] * a.y + aa[2]) / s;
//		aaa.y = (aa[3] * a.x + aa[4] * a.y + aa[5]) / s;
//		dst.push_back(aaa);
//	}
//	origin = dst;
//}
//
//
//int main()
//{
//	Ptr<cv::ORB> orb = ORB::create();
//	Mat imga, imgb;
//	VideoCapture vc(1);
//	vc >> imga;
//	Ptr<Fast> fast = new Fast(10, 500, imga.size());
//
//
//	cvWaitKey(50);
//
//
//
//
//	char c;
//	bool isSet = false;
//
//	vector<KeyPoint> kptsa, kptsb;
//	Mat despa, despb;
//	Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create("BruteForce-Hamming");
////	GetMatch getMatch = GetMatch(dist_ratioCheck);
//	Marker m;
//	warpSingleStruct last;
//	warpSingleStruct now;
//	vector<vector<DMatch>> dmatches;
//	vector<DMatch> dmatch;
//
//	bool isStart = false;
//	while (c = cvWaitKey(30))
//	{
//		Mat temp;
//		vc >> temp;
//		now.structure.img = temp;
//		if (c == 'a')
//		{
//			m = Marker(temp, Mat(), fast, orb);
//			isSet = true;
//			continue;
//		}
//		if (isSet)
//		{
//			if (!isStart)
//			{
//
//				orb->detectAndCompute(now.structure.img, Mat(), now.structure.kpts, now.structure.desp);
//				matcher->knnMatch(now.structure.desp, m.orbStructure.desp, dmatches, 2);
//				vector<int> idx;
//				GetMatch::GetInliers(dmatches, idx,0.6,90);
//				GetMatch::GetDMatch(dmatches, idx, dmatch);
////				GetMatch::GetMaskObj(dmatch, idx);
//				vector<Point2f> query, train;
//				GetMatch::preparePoint2D(now.structure.kpts, m.orbStructure.kpts, dmatch, query, train);
//				Mat mask;
//				Mat H = findHomography(query, train, mask, CV_RANSAC);
//				double t = scaleFromHomography(H);
//				int step = log(t) / log(1.25);
//
//				warpPerspective(m.fastStructure.imagePyramid[step], last.structure.img, H, m.img.size());
//				TickMeter tm;
//				tm.start();
//				fast->detect(last.structure.img, last.structure.kpts);
//				tm.stop();
//				cout << "detect:" << tm.getTimeMicro() << endl;
//				tm.reset();
//				tm.start();
//				fast->compute(last.structure.img, last.structure.kpts, last.structure.desp);
//				tm.stop();
//				cout << "compute:" << tm.getTimeMicro() << endl;
//
//				fast->detect(now.structure.img, now.structure.kpts);
//				fast->compute(now.structure.img, now.structure.kpts, now.structure.desp);
//				tm.reset();
//				tm.start();
//				fast->match(now.structure.img, now.structure.kpts, now.structure.desp, last.structure.kpts, last.structure.desp, dmatch);
//				tm.stop();
//				cout << "match:" << tm.getTimeMicro() << endl;
//				GetMatch::GetInliers(dmatch, idx,90);
//				GetMatch::preparePoint2D(now.structure.kpts, last.structure.kpts, dmatch, query, train);
//				findHomography(query, train, mask, CV_RANSAC);
//				GetMatch::GetMaskObj(query, mask);
//				GetMatch::GetMaskObj(train, mask);
//				transformPointsInverse(train, H);
//				last.H = H.clone();
//				temp = drawTrackLine(temp, train, query);
//				isStart = true;
//			}
//			else
//			{
//				vector<int> idx;
//				vector<Point2f> query, train;
//				Mat mask;
//				TickMeter tm;
//				tm.start();
//				fast->detect(now.structure.img, now.structure.kpts,last.structure.img);
//				tm.stop();
//				cout << "detect:" << tm.getTimeSec() << endl;
//				tm.reset();
//				tm.start();
//				fast->compute(now.structure.img, now.structure.kpts, now.structure.desp);
//				tm.stop();
//				cout << "compute:" << tm.getTimeSec() << endl;
//				tm.reset();
//				tm.start();
//				fast->match(now.structure.img, now.structure.kpts, now.structure.desp, last.structure.kpts, last.structure.desp, dmatch);
//				tm.stop();
//				cout << "match:" << tm.getTimeSec() << endl;
//				GetMatch::GetInliers(dmatch, idx, 150);
//				GetMatch::preparePoint2D(now.structure.kpts, last.structure.kpts, dmatch, query, train);
//				transformPointsInverse(train, last.H);
//				Mat H = findHomography(query,train,mask,CV_RANSAC);
//				double t = scaleFromHomography(H);
//				int step = log(t) / log(1.25);
//				if (step < 0)
//					step = 0;
//				warpPerspective(m.fastStructure.imagePyramid[step], last.structure.img, H, m.img.size());
//				fast->detect(last.structure.img, last.structure.kpts);
//				fast->compute(last.structure.img, last.structure.kpts, last.structure.desp);
//				fast->match(now.structure.img, now.structure.kpts, now.structure.desp, last.structure.kpts, last.structure.desp, dmatch);
//				GetMatch::GetInliers(dmatch, idx, 90);
//				GetMatch::preparePoint2D(now.structure.kpts, last.structure.kpts, dmatch, query, train);
//				findHomography(query, train, mask, CV_RANSAC);
//				GetMatch::GetMaskObj(query, mask);
//				GetMatch::GetMaskObj(train, mask);
//				transformPointsInverse(train, H);
//				last.H = H.clone();
//				temp = drawTrackLine(temp, train, query);
//			}
//
//		}
//		imshow("now", temp);
//	}
//}
//
